#include "mloop.h"

#define CLOSURE_THRESHOLD 0.08

/* return value: select1 : resno select2 : phi(1)/psi(2) angle: rotate angle */
float select_residue2(struct candidate *p, int segid, int *select1, int *select2, float *angle, int avoidance, int avoidance1, int avoidance2)
{
	float axis[3], a1[3], a2[3], x1[3], x2[3], da1, da2, dx1, dx2;
	float c1[LOOPRES][2], c2[LOOPRES][2], c3[LOOPRES][2];
	float dmin, dist, alpha, theta;
	int i, j;

	dmin = 9999.0;
	for (i = loop.seg_start[segid]; i < loop.seg_end[segid]; i++)
	{
		for (j = 0; j < 2; j++)
		{
			float tmp;

			if (avoidance == i)
				continue;
			if (avoidance1 == i && avoidance2 == j + 1)
				continue;
			axis[0] = p->coor[loop.start[i] + j + 1][0] - p->coor[loop.start[i] + j][0];
			axis[1] = p->coor[loop.start[i] + j + 1][1] - p->coor[loop.start[i] + j][1];
			axis[2] = p->coor[loop.start[i] + j + 1][2] - p->coor[loop.start[i] + j][2];
			tmp = sqrtf (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
			axis[0] = axis[0] / tmp;
			axis[1] = axis[1] / tmp;
			axis[2] = axis[2] / tmp;

			a1[0] = p->dummy_anchor[segid].N[0] - p->coor[loop.start[i] + j][0];
			a1[1] = p->dummy_anchor[segid].N[1] - p->coor[loop.start[i] + j][1];
			a1[2] = p->dummy_anchor[segid].N[2] - p->coor[loop.start[i] + j][2];
			a2[0] = p->dummy_anchor[segid].CA[0] - p->coor[loop.start[i] + j][0];
			a2[1] = p->dummy_anchor[segid].CA[1] - p->coor[loop.start[i] + j][1];
			a2[2] = p->dummy_anchor[segid].CA[2] - p->coor[loop.start[i] + j][2];

			x1[0] = loop.anchor[segid].N[0] - p->coor[loop.start[i] + j][0];
			x1[1] = loop.anchor[segid].N[1] - p->coor[loop.start[i] + j][1];
			x1[2] = loop.anchor[segid].N[2] - p->coor[loop.start[i] + j][2];
			x2[0] = loop.anchor[segid].CA[0] - p->coor[loop.start[i] + j][0];
			x2[1] = loop.anchor[segid].CA[1] - p->coor[loop.start[i] + j][1];
			x2[2] = loop.anchor[segid].CA[2] - p->coor[loop.start[i] + j][2];

			da1 = a1[0] * a1[0] + a1[1] * a1[1] + a1[2] * a1[2];
			da2 = a2[0] * a2[0] + a2[1] * a2[1] + a2[2] * a2[2];
			dx1 = x1[0] * x1[0] + x1[1] * x1[1] + x1[2] * x1[2];
			dx2 = x2[0] * x2[0] + x2[1] * x2[1] + x2[2] * x2[2];

			c1[i][j] = (axis[0] * a1[0] + axis[1] * a1[1] + axis[2] * a1[2]) * (axis[0] * x1[0] + axis[1] * x1[1] + axis[2] * x1[2]) + (axis[0] * a2[0] + axis[1] * a2[1] + axis[2] * a2[2]) * (axis[0] * x2[0] + axis[1] * x2[1] + axis[2] * x2[2]);
			c2[i][j] = a1[0] * x1[0] + a1[1] * x1[1] + a1[2] * x1[2] + a2[0] * x2[0] + a2[1] * x2[1] + a2[2] * x2[2] - c1[i][j];
			c3[i][j] = x1[0] * (axis[1] * a1[2] - axis[2] * a1[1]) + x1[1] * (axis[2] * a1[0] - axis[0] * a1[2]) + x1[2] * (axis[0] * a1[1] - axis[1] * a1[0]) + x2[0] * (axis[1] * a2[2] - axis[2] * a2[1]) + x2[1] * (axis[2] * a2[0] - axis[0] * a2[2]) + x2[2] * (axis[0] * a2[1] - axis[1] * a2[0]);
			dist = da1 + da2 + dx1 + dx2 - 2.0 * (c1[i][j] + sqrtf (c2[i][j] * c2[i][j] + c3[i][j] * c3[i][j]));
			if (dist > dmin)
				continue;
			alpha = atanf ( c3[i][j] / c2[i][j]) * RADIAN;
			if (c2[i][j] < 0)
			{
				alpha += 180.0;
				alpha = normal_angle (alpha);
			}
			theta = allow_angle (p->phi[i], p->psi[i], loop.type[i], j, alpha);
			dist = da1 + da2 + dx1 + dx2 - 2.0 * (c1[i][j] + sqrtf (c2[i][j] * c2[i][j] + c3[i][j] * c3[i][j]) * cosf (theta / RADIAN));
			if (dist < dmin)
			{
				dmin = dist;
				*select1 = i;
				*select2 = j;
				alpha += theta;
				alpha = normal_angle (alpha);
				*angle = alpha;
				if (dmin < CLOSURE_THRESHOLD)
					break;
			}
		}
	}

	*select2 = *select2 + 1;
	return dmin;
}

/* return value: 0: ok -1: fail */
int loop_closure2(struct candidate *p, int avoidance, int threshold, int segid)
{
	float angle, phi, psi;
	float dist;
	int select1, select2, rot_flag[LOOPNUM], n;
	int avoidance1, avoidance2;

	n = 0;
	avoidance1 = avoidance2 = -1;
	do
	{
		dist = select_residue2 (p, segid, &select1, &select2, &angle, avoidance, avoidance1, avoidance2);
		avoidance1 = select1;
		avoidance2 = select2;
		set_flag (segid, select1, select2, rot_flag);
		if (select2 == 1)
		{
			phi = p->phi[select1] + angle;
			phi = normal_angle (phi);
			p->phi[select1] = phi;
			rotate_atoms (p->coor[loop.start[select1]], p->coor[loop.start[select1] + 1], p, segid, rot_flag, angle);
		}
		else
		{
			psi = p->psi[select1] + angle;
			psi = normal_angle (psi);
			p->psi[select1] = psi;
			rotate_atoms (p->coor[loop.start[select1] + 1], p->coor[loop.start[select1] + 2], p, segid, rot_flag, angle);
		}

		if (dist < CLOSURE_THRESHOLD)
			break;
		n++;
		/*
		if (n == 2)
			avoidance = -1;
		*/
	}
	while (n < threshold);

	if (n < threshold)
		return 0;
	else
		return -1;
}
